package com.googlecode.grs.robot;

import java.awt.geom.Point2D;
import java.awt.geom.Point2D.Double;
import java.util.ArrayList;

import com.googlecode.grs.core.GRTObject;
import com.googlecode.grs.core.Mechanism;
import com.googlecode.grs.environment.SimulationEnvironment;

/**
 * A MovingRobot tracks a robot's position, given its speed and heading. It also
 * keeps a history of places its been, providing a path.
 * 
 * @author ajc
 * 
 */
public class SimpleEventMoveRobot extends GRTObject implements Moveable,
		Mechanism {

	public static final double TIMESTEP = 0.01; // update 100 times a second
	private static final int MAX_SPEED = 100;

	// Command constants
	private static final int FORWARDS = +1;
	private static final int BACKWARDS = -1;
	private static final int LEFT = -1; // ccw
	private static final int RIGHT = +1; // cw
	private static final int STOPPED = 0;

	// POSITION
	protected double x, y;// 2D position
	protected double heading;// 2D orientation
	private SimulationEnvironment env;// position of environment objects

	// COMMANDS
	protected int direction = STOPPED; // current commanded direction
	protected int rot_direction = STOPPED; // current commanded angular
											// direction

	// RATES
	protected int speed = MAX_SPEED; // rate of forward movement
	protected double omega = Math.PI / 4.;// rate of turning

	// TIME
	public static double time = 0.0;

	// list of all points robot has been
	private ArrayList<Point2D.Double> path = new ArrayList<Point2D.Double>();

	private boolean waiting = false; // true if moving, or waiting for an events
	private boolean running = false; // true if calculating position (running
										// thread)

	/**
	 * Moving robot default ctor. Analogous to Simulator mainloop thread(the
	 * calculating position component).
	 * 
	 * This is called by the EnvironmentFactory. It requires an original
	 * position.
	 * 
	 * @param x
	 *            x component of 2D position
	 * @param y
	 *            y component of 2D position
	 * @param heading
	 *            2D orientation
	 * 
	 */
	public SimpleEventMoveRobot(SimulationEnvironment env, double x, double y,
			double heading) {
		this.env = env;
		this.x = x;
		this.y = y;
		this.heading = heading;
		path.add(new Point2D.Double(x, y));
	}

	/**
	 * Starts calculating position based on commands
	 */
	public void startCalculation() {
		start();
	}

	/**
	 * Calculates new position and saves a history of points on a regular clock.
	 * When higher level operations are called (goForward/Backward,
	 * turnLeft/Right), they change robot movement on a regular clock(here)
	 */
	synchronized public void run() {
		running = true;
		while (running) {
			// TODO calculate movement
			if (waiting) {
				x += speed * Math.cos(heading) * direction * TIMESTEP;
				y += speed * Math.sin(heading) * direction * TIMESTEP;
				heading += omega * rot_direction * TIMESTEP;
			}

			// physically stop any moving through walls
			// TODO: allow dragging across walls
			for (int i = 0; i < env.getObjects().size(); i++) {
				while (env.get(i).intersects(x, y)) {
					x -= speed * Math.cos(heading) * direction * TIMESTEP;
					y -= speed * Math.sin(heading) * direction * TIMESTEP;
				}
			}

			if ((int) (time * (1 / TIMESTEP)) % (int) (10 / TIMESTEP) == 0) {
				System.out.println("time: " + (int) time);
			}
			if ((int) (time * (1 / TIMESTEP)) % (int) (.1 / TIMESTEP) == 0) {
				path.add(new Point2D.Double(x, y));
			}

			time += TIMESTEP;

			// hand off to another thread
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}

	}

	/*
	 * ******************************
	 * Higher level operations: used by mechanisms
	 */

	/**
	 * Commands this robot to drive forward at maximum velocity. Does not
	 * require a waitDistance call to immediately take effect. If turning, the
	 * robot will stop doing so and being goingForward()
	 */
	public void goForward() {
		direction = FORWARDS;
		rot_direction = STOPPED;
	}

	/**
	 * Commmands robot movement opposite of goForward()
	 */
	public void goBackward() {
		direction = BACKWARDS;
		rot_direction = STOPPED;
	}

	/**
	 * Commands the robot to make a point turn to the left at maximum velocity
	 * If the robot is goingForward or backwards it will stop doing so.
	 */
	public void turnLeft() {

		direction = STOPPED;
		rot_direction = LEFT;
	}

	/**
	 * Commands the robot to make a point turn to the right at maximum velocity
	 * If the robot is goingForward or backwards it will stop doing so.
	 */
	public void turnRight() {

		direction = STOPPED;
		rot_direction = RIGHT;
	}

	/**
	 * Hangs execution of code until the MovingRobot has moved a certain
	 * distance
	 * 
	 * @condition distance > 0
	 * @param distance
	 *            distance to move
	 */
	public void waitDistance(int distance) {
		if (distance < 0) {
			new Throwable("Cannot wait negative distance: entered: " + distance)
					.printStackTrace();
			// try {
			// throw new Exception("negative distance");
			// } catch (Exception e) {
			// // TODO Auto-generated catch block
			// e.printStackTrace();
			// }
		}
		waiting = true;
		Point2D.Double start = new Point2D.Double(x, y);
		while (start.distance(x, y) <= distance) {
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch blocks
				e.printStackTrace();
			}
		}
		waiting = false;
	}

	/**
	 * Hangs execution of code until the MovingRobot has turned a certain angle
	 * 
	 * @condition angle > 0
	 * @param angle
	 */
	public void waitAngle(int angle) {
		if (angle < 0) {
			new Throwable("Cannot wait negative angle: entered: " + angle)
					.printStackTrace();
			// try {
			// throw new Exception("negative distance");
			// } catch (Exception e) {
			// // TODO Auto-generated catch block
			// e.printStackTrace();
			// }
		}
		// convert to radians
		double radangle = Math.toRadians(angle);
		waiting = true;
		double startHeading = heading;
		// hang until the differnce exceeds angle
		while (Math.abs(heading - startHeading) <= radangle) {
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		waiting = false;
	}

	/**
	 * Stops all actuation.
	 */
	public void halt() {

		direction = STOPPED;
		rot_direction = STOPPED;
	}

	/*
	 * *****************************
	 * Position: used by painters
	 */

	public ArrayList<Point2D.Double> getPath() {
		return path;
	}

	/*
	 * ******************************
	 * MOveable implementation
	 */

	@Override
	public double getX() {
		// TODO Auto-generated method stub
		// TODO use Point2D.Double?
		return x;
	}

	@Override
	public double getY() {
		// TODO Auto-generated method stub
		return y;
	}

	@Override
	public double getHeading() {
		// TODO Auto-generated method stub
		return heading;
	}

	/**
	 * @deprecated we start with position: however this may be useful for mouse
	 *             dragging
	 */
	public void setPosition(double x, double y, double angle) {
		// TODO Auto-generated method stub

	}

	/**
	 * @deprecated we start with postion: nobody can move us except by mouse
	 *             drag
	 */
	public void setPosition(Double position, double angle) {
		// TODO Auto-generated method stub

	}

	/*
	 * ***************************************
	 * MECHANISM (non-Javadoc)
	 * 
	 * @see com.grt192.core.Mechanism#pause()
	 */

	@Override
	public void pause() {

	}

	@Override
	public void unPause() {

	}
}
